#include <stdint.h>
#include <stddef.h>
#include <iomacros.h>
#include <msp430f2617.h>

#include "drivers/uart.h"
#include "drivers/dbg_uart.h"
#include "receive.h"
#include "fmux.h"
#include "error.h"


/*static const uint8_t *txBuffer = NULL;
static uint16_t txBufferLength = 0;*/

uint8_t cmd_task;

/**
 *     +------+ +------+ +------+ +------+
 *     | xxxx | | char | | char | | xxxx |
 * ... +------+ +------+ +------+ +------+ ...
 *	               ^                 ^
 *	             tail               head
 */

static struct {
	uint8_t data[UART_RECV_BUFFER_SIZE];
	// The index of the next free location
	uint16_t head;
	// The index of the last written char
	uint16_t tail;
} rx_buffer;

static struct {
	uint8_t data[UART_SEND_BUFFER_SIZE];
	// The index of the next free location
	uint16_t head;
	// The index of the last written char
	uint16_t tail;
} tx_buffer;

/* Gets the remaining FREE bytes in the buffer */
static inline uint16_t tx_buf_get_free_bytes(void)
{
	uint16_t free = 0;
	uint16_t tail_tmp = tx_buffer.tail;

	if (tx_buffer.head >= tail_tmp)
	{
		free = UART_SEND_BUFFER_SIZE - (tx_buffer.head - tail_tmp);
	}
	else
	{
		free = tail_tmp - tx_buffer.head;
	}

	return free;
}

/**
 * Assembles a command
 */
static struct {
	uint8_t data[CMD_RECV_BUFFER_SIZE];
	// The index of the next free location
	uint8_t head;
} cmd_buffer;

union cmd_line_t cmd_line; 


void uart_enable(void)
{
	/* Enable CTS (2.4) pin */
	P2SEL &= ~BV(4);
	P2DIR |= BV(4);
	P2OUT &= ~BV(4);
	/* Enable rx/tx pins */
	P3SEL |= 0x30;
	/* 8N1 */
	UCA0CTL0 = 0x00;
	/* Select clock source: SMCLK */
	UCA0CTL1 = UCSSEL_2;
	/* Select 115200 Baud under 8MHZ */
	UCA0BR0 = 0x45;//0x44;
	UCA0BR1 = 0x00;
	/* Modulation */
	UCA0MCTL = (UCBRS_4 | UCBRF_0);
	/* Switch on */
	UCA0CTL1 &= ~UCSWRST;
	/* Enable rx interrupt */
	IE2 |= UCA0RXIE;

	rx_buffer.head = 0;
	rx_buffer.tail = 0;

	cmd_buffer.head = 0;
}

void uart_disable(void)
{
	/* Switch off */
	UCA0CTL1 |= UCSWRST;
	/* Disable rx interrupt */
	IE2 &= ~UCA0RXIE;
}

uint8_t uart_send(const uint8_t *msg, uint16_t len)
{
	/*if (txBuffer != NULL) {
		return 0;
	}

	txBuffer = aBuf;
	txBufferLength = aBufLength;
	*/

	if (tx_buf_get_free_bytes() <= (len + 1)) {
		error("TX Full\n");
		return 0;
	}

	for (uint16_t i = 0; i < len; i++) {
		tx_buffer.data[tx_buffer.head] = msg[i];
		tx_buffer.head = (tx_buffer.head + 1) % UART_SEND_BUFFER_SIZE;
	}

	IE2 |= UCA0TXIE;

	return 1;
}

uint16_t uart_receive(uint8_t *r_buf, uint16_t r_buf_len)
{
	uint16_t i = 0;

	while (i < r_buf_len) {
		cmd_buffer.data[cmd_buffer.head++] = r_buf[i++];

		if (cmd_buffer.head+1 == CMD_RECV_BUFFER_SIZE) {
			error("CMD too long");
		}

		// cmd_buffer.data[0] contains the length of the data
		if ((cmd_buffer.head > 1) &&
			(cmd_buffer.head == (cmd_buffer.data[0] + sizeof(struct comm_unit))) &&
			1 ) {
			//(cmd_task == 0)) {
			for (uint16_t j = 0; j < cmd_buffer.head; j++) {
				cmd_line.cmd_raw[j] = cmd_buffer.data[j];
			}
			cmd_buffer.head = 0;

			cmd_task = 1;
			recv_process(&(cmd_line.cmd));
			break;
		}
	}
	return i;
}

void process_receive(void)
{
	// Copy tail to prevent multiple reads
	uint16_t head = rx_buffer.head;

	// If the data wraps around, process the first part
	while (rx_buffer.tail > head) {
		rx_buffer.tail = (rx_buffer.tail +
			uart_receive(
				&(rx_buffer.data[rx_buffer.tail]),
				UART_RECV_BUFFER_SIZE - rx_buffer.tail))
			% UART_RECV_BUFFER_SIZE;
	}

	// For any data remaining, process it
	while (rx_buffer.tail != head) {
		rx_buffer.tail +=
			uart_receive(
				&(rx_buffer.data[rx_buffer.tail]),
				head - rx_buffer.tail);
	}

	// If the cts pin was asserted, release it
	if (P2OUT & BV(4)) {
		P2OUT &= ~BV(4);
	}
}

ISR(USCIAB0TX_VECTOR)
{
	if (tx_buf_get_free_bytes() != UART_SEND_BUFFER_SIZE) {
		UCA0TXBUF = tx_buffer.data[tx_buffer.tail];
		tx_buffer.tail = (tx_buffer.tail + 1) % UART_SEND_BUFFER_SIZE;
	} else {
		/* Transmit finished turn isr off */
		IE2 &= ~UCA0TXIE;
	}
}

ISR(USCIAB0RX_VECTOR)
{
	uint8_t byte = UCA0RXBUF;
	// TODO: check for cts low
	// We can only write if incrementing head doesn't equal tail
	//  (+ 2 + 1) because we need to save the byte we currently have,
	//  leave one byte between head & tail and, if the sender is
	//  sending fast, then the next byte is already on its way.
	//  (i.e. we need to store it because the host wont resend it despite cts)
	if ((rx_buffer.head + 2 + 1) % UART_RECV_BUFFER_SIZE != rx_buffer.tail) {
		rx_buffer.data[rx_buffer.head] = byte;
		rx_buffer.head = (rx_buffer.head + 1) % UART_RECV_BUFFER_SIZE;
	} else {
		rx_buffer.data[rx_buffer.head] = byte;
		rx_buffer.head = (rx_buffer.head + 1) % UART_RECV_BUFFER_SIZE;
		// and pull cts pin high
		P2OUT |= BV(4);
	}
}
